#ifndef WHEELODOM_H
#define WHEELODOM_H

#include <Eigen/Core> //Eigen库的核心模块，它提供了Matrix和Array两种模板类，以及基础的线性代数运算和数组操作
#include <Eigen/Geometry> //Eigen库的几何模块，它提供了各种几何变换的类和函数，例如旋转，平移，缩放，二维和三维的变换矩阵，欧拉角，四元数等。它可以用于处理三维图形或机器人学等领域的问题
#include <memory> //C++标准库中的一个头文件，它提供了一些智能指针的类和函数

#include "State.h"
#include "../Converter.h"

namespace msf
{

constexpr int kMeasDim = 3;

struct OdomData
{
      double timestamp;

      Eigen::Vector3d position; // x y z (m)
      Eigen::Vector3d attitude; // roll pitch yaw (rad)
      Eigen::Vector3d linear_vel; // vx vy vz (m/s)
      Eigen::Vector3d angular_vel; // wx wy wz (rad/s)

      Eigen::Matrix<double,15,15> cov;  // Covariance
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

//using OdomDataPtr = std::shared_ptr<OdomData>;
//using OdomDataConstPtr = std::shared_ptr<const OdomData>;

class WheelOdom {
 public:
      WheelOdom() = default; // 告诉编译器为该类生成一个默认的构造函数

      ~WheelOdom() {} // 虚析构函数，作用是在通过基类指针删除派生类对象时,能够正确地调用派生类的析构函数,避免资源泄漏或未定义行为

      static Eigen::MatrixXd measurement_function(const Eigen::MatrixXd &mat_x) { // 用virtual的原因是可能这个函数是想要被其他派生类重写的，或者这个函数本身就是从其他基类继承而来的。如果不用virtual，那么这个函数就不能被重写，也不能实现多态
            Eigen::Isometry3d Twb;
            Twb.matrix() = mat_x;
            return Twb * I_p_odom_;
      }      

      // TODO
      static Eigen::MatrixXd measurement_residual(const Eigen::MatrixXd &mat_x, const Eigen::MatrixXd &mat_z) {
            return mat_z - measurement_function(mat_x);
      }      

      // TODO
      static Eigen::MatrixXd measurement_jacobian(const Eigen::MatrixXd &mat_x, const Eigen::MatrixXd &mat_z) {
            Eigen::Isometry3d Twb;
            Twb.matrix() = mat_x;

            Eigen::Matrix<double, kMeasDim, kStateDim> H;
            H.setZero();
            H.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity();
            H.block<3, 3>(0, 6) = -Twb.linear() * Converter::skew_matrix(I_p_odom_);

            return H;
      }
      
      /**
       * @brief global to local coordinate, convert to frame
       *
       * @param odom_data_ptr
       * @return Eigen::Vector3d
       */
      static Eigen::Vector3d g2l(OdomData odom_data_ptr) {
            Eigen::Vector3d p_G_Odom;
            return p_G_Odom;
      }

 private:
      static Eigen::Vector3d I_p_odom_;
      Eigen::Vector3d init_pos_;
};
using OdomPtr = std::shared_ptr<WheelOdom>;

//Eigen::Vector3d WheelOdom::I_p_odom_ = Eigen::Vector3d::Zero(); // 初始化一个非整型的静态数据成员，需要在类外进行初始化 // 静态成员变量的定义和初始化应该放在源文件中，而不是头文件中，以避免重复定义的问题

} // namespace msf
#endif